import re
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.animation import FuncAnimation
from scipy.interpolate import interp1d

# ==================== 配置文件路径 ====================
log_path = r"c:\Users\LSQ_CX\Desktop\下午日志详细分析\control_node_3154_64123.log"

# 存储数据
mydrone_data = []
estimate_data = []

# 正则表达式
pattern_mode_switch = re.compile(r"手动切换模式模式_报文模式")  # 触发标志
pattern_time = re.compile(r"cur_ekf_time:\s*([-\d.eE]+)")
pattern_estimate = re.compile(r"estimate_info:\s*\[([-\d.eE+]+),\s*([-\d.eE+]+),\s*([-\d.eE+]+)")
pattern_mydrone = re.compile(r"mydrone_enu:\s*\[\s*([-\d.eE+-]+)\s+([-\d.eE+-]+)\s+([-\d.eE+-]+)")

current_time = None
start_recording = False  # 控制是否开始记录数据

# ==================== 第一遍扫描：找到“手动切换模式_报文模式”首次出现后开始解析 ====================
with open(log_path, 'r', encoding='utf-8') as f:
    for line in f:
        # 检查是否触发了模式切换
        if not start_recording and pattern_mode_switch.search(line):
            print(f"检测到‘手动切换模式_报文模式’，从此行后开始记录数据：")
            print(f" -> {line.strip()}")
            start_recording = True
            continue  # 这一行本身不处理数据，下一行开始记录

        # 只有在触发后才处理数据
        if start_recording:
            # 更新时间戳
            time_match = pattern_time.search(line)
            if time_match:
                current_time = float(time_match.group(1))

            # 提取自身无人机
            drone_match = pattern_mydrone.search(line)
            if drone_match and current_time is not None:
                x, y, z = map(float, drone_match.groups())
                mydrone_data.append((current_time, x, y, z))

            # 提取靶机估计
            est_match = pattern_estimate.search(line)
            if est_match and current_time is not None:
                x, y, z = map(float, est_match.groups())
                estimate_data.append((current_time, x, y, z))

# 转为 NumPy 数组
mydrone_data = np.array(mydrone_data)
estimate_data = np.array(estimate_data)

if len(mydrone_data) == 0 or len(estimate_data) == 0:
    print("在‘手动切换模式_报文模式’后未提取到足够数据！")
    exit()

print(f"✅ 在‘手动切换模式_报文模式’后提取到：")
print(f"   - 自身无人机数据点: {len(mydrone_data)}")
print(f"   - 靶机估计数据点: {len(estimate_data)}")

# ==================== 时间对齐与插值（同前） ====================
t_min = max(mydrone_data[0, 0], estimate_data[0, 0])
t_max = min(mydrone_data[-1, 0], estimate_data[-1, 0])

mydrone_valid = mydrone_data[(mydrone_data[:, 0] >= t_min) & (mydrone_data[:, 0] <= t_max)]
estimate_valid = estimate_data[(estimate_data[:, 0] >= t_min) & (estimate_data[:, 0] <= t_max)]

if len(mydrone_valid) == 0 or len(estimate_valid) == 0:
    print("无公共时间区间！")
    exit()

# 统一时间轴
fps = 30
duration = t_max - t_min
num_frames = int(duration * fps)
t_common = np.linspace(t_min, t_max, num_frames)

# 插值函数
def safe_interp(data_txyz, t_new):
    f_x = interp1d(data_txyz[:, 0], data_txyz[:, 1], kind='linear', fill_value="extrapolate")
    f_y = interp1d(data_txyz[:, 0], data_txyz[:, 2], kind='linear', fill_value="extrapolate")
    f_z = interp1d(data_txyz[:, 0], data_txyz[:, 3], kind='linear', fill_value="extrapolate")
    return np.column_stack([f_x(t_new), f_y(t_new), f_z(t_new)])

mydrone_interp = safe_interp(mydrone_valid, t_common)
estimate_interp = safe_interp(estimate_valid, t_common)
distances = np.linalg.norm(mydrone_interp - estimate_interp, axis=1)

# ==================== 动画部分（同前） ====================
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')

drone_point, = ax.plot([], [], [], 'ro', markersize=8, label='Own Drone')
target_point, = ax.plot([], [], [], 'bo', markersize=8, label='Target Drone')
connection_line, = ax.plot([], [], [], 'k--', linewidth=1, alpha=0.7)
distance_text = ax.text2D(0.02, 0.95, '', transform=ax.transAxes, fontsize=12,
                          bbox=dict(boxstyle="round", facecolor="wheat"))

# 坐标轴范围
all_x = np.concatenate([mydrone_interp[:, 0], estimate_interp[:, 0]])
all_y = np.concatenate([mydrone_interp[:, 1], estimate_interp[:, 1]])
all_z = np.concatenate([mydrone_interp[:, 2], estimate_interp[:, 2]])

padding = 0.05
ax.set_xlim(all_x.min() * (1 - padding), all_x.max() * (1 + padding))
ax.set_ylim(all_y.min() * (1 - padding), all_y.max() * (1 + padding))
ax.set_zlim(all_z.min() * (1 - padding), all_z.max() * (1 + padding))

ax.set_xlabel("Easting (X)")
ax.set_ylabel("Northing (Y)")
ax.set_zlabel("Height (Z)")
ax.set_title("Post-Switch Tracking Animation")
ax.legend()

# ==================== 动画更新函数 ====================
def update(frame):
    my_x, my_y, my_z = mydrone_interp[frame]
    tg_x, tg_y, tg_z = estimate_interp[frame]
    dist = distances[frame]

    drone_point.set_data([my_x], [my_y])
    drone_point.set_3d_properties([my_z])

    target_point.set_data([tg_x], [tg_y])
    target_point.set_3d_properties([tg_z])

    connection_line.set_data([my_x, tg_x], [my_y, tg_y])
    connection_line.set_3d_properties([my_z, tg_z])

    distance_text.set_text(f'Distance: {dist:.2f} m\nTime: {t_common[frame] - t_common[0]:.1f}s')

    return drone_point, target_point, connection_line, distance_text

# ==================== 创建动画 ====================
ani = FuncAnimation(
    fig, update, frames=num_frames,
    interval=1000/fps,      # 可调速：减小此值加快播放
    blit=False,
    repeat=True
)

plt.tight_layout()
plt.show()